function [edge] = edge_ellipse(I, edges0, coef, nNeighborPts, grayScaleDiffMethod, gaussFitMethod) %#codegen
%EDGE_ELLIPSE 寻找椭圆细分边缘
%
% Input Arguments
% # I: uint8矩阵，图像矩阵
% # edges0: m*2*p数组，m为最长椭圆边缘的点数，p为椭圆数，较短边缘靠后行可以用小于0的数值填充，单位像素
% # coef: m*6矩阵，椭圆方程系数，A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
% # nNeighborPts: 标量，拟合时使用的相邻点个数，默认为3
% # grayScaleDiffMethod: int8标量，计算灰度差的算法，1为论文算法，其它为使用相邻元素计算灰度差
% # gaussFitMethod: int8标量，拟合高斯曲线的算法，1为论文算法，其它为非线性最小二乘进行拟合
%
% Output Arguments
% # edge: 与edges0相同尺寸的数组，细分边缘
%
% Algorithms
% # \[\begin{align}
%   & \operatorname{n}\left( t \right)=\frac{1}{\sqrt{2\pi }}{{e}^{-\frac{{{t}^{2}}}{2}}} \\
%  & \int_{0}^{x}{\operatorname{n}\left( t \right)\operatorname{d}t}=\int_{0}^{x}{\frac{1}{\sqrt{2\pi }}{{e}^{-\frac{{{t}^{2}}}{2}}}\operatorname{d}t}=\frac{1}{\sqrt{2\pi }}\int_{0}^{x}{{{e}^{-{{{{t}'}}^{2}}}}\operatorname{d}\left( \sqrt{2}{t}' \right)}=\frac{1}{\sqrt{\pi }}\int_{0}^{\frac{x}{\sqrt{2}}}{{{e}^{-{{{{t}'}}^{2}}}}\operatorname{d}{t}'}=\frac{1}{2}\operatorname{erf}\left( \frac{x}{\sqrt{2}} \right) \\
%  & \int_{-\infty }^{-x}{\operatorname{n}\left( t \right)\operatorname{d}t}=\int_{x}^{+\infty }{\operatorname{n}\left( t \right)\operatorname{d}t}=\int_{x}^{+\infty }{\frac{1}{\sqrt{2\pi }}{{e}^{-\frac{{{t}^{2}}}{2}}}\operatorname{d}t}=\frac{1}{\sqrt{2\pi }}\int_{x}^{+\infty }{{{e}^{-{{{{t}'}}^{2}}}}\operatorname{d}\left( \sqrt{2}{t}' \right)}=\frac{1}{\sqrt{\pi }}\int_{\frac{x}{\sqrt{2}}}^{+\infty }{{{e}^{-{{{{t}'}}^{2}}}}\operatorname{d}{t}'}=\frac{1}{2}\operatorname{erfc}\left( \frac{x}{\sqrt{2}} \right) \\
%  & \operatorname{r}\left( u \right)=\left\{ \begin{matrix}
%    0 & \left( u<{{u}_{r}} \right)  \\
%    1 & \left( u\ge {{u}_{r}} \right)  \\
% \end{matrix} \right. \\
%  & \operatorname{i}\left( x \right)=\int_{-\infty }^{+\infty }{\operatorname{r}\left( u \right)a\cdot \operatorname{n}\left( \frac{x-u}{\sigma } \right)\operatorname{d}u}=a\int_{{{u}_{r}}}^{+\infty }{\operatorname{n}\left( \frac{x-u}{\sigma } \right)\operatorname{d}u} \\
%  & =\left\{ \begin{matrix}
%    a\int_{{{u}_{r}}}^{+\infty }{\operatorname{n}\left( -{u}' \right)\operatorname{d}\left( x+\sigma {u}' \right)}=a\sigma \int_{\frac{{{u}_{r}}-x}{\sigma }}^{+\infty }{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}=\frac{a\sigma }{2}\operatorname{erfc}\left( \frac{{{u}_{r}}-x}{\sqrt{2}\sigma } \right)  \\
%    a\int_{{{u}_{r}}}^{+\infty }{\operatorname{n}\left( {{u}'} \right)\operatorname{d}\left( x-\sigma {u}' \right)}=a\sigma \int_{-\infty }^{\frac{x-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}  \\
% \end{matrix} \right. \\
%  & {{i}_{k}}=\int_{k-0.5}^{k+0.5}{\operatorname{i}\left( x \right)\operatorname{d}x} \\
%  & {{i}_{k+1}}-{{i}_{k}}=\int_{k+0.5}^{k+1.5}{\operatorname{i}\left( x \right)\operatorname{d}x}-\int_{k-0.5}^{k+0.5}{\operatorname{i}\left( x \right)\operatorname{d}x}=a\sigma \int_{k+0.5}^{k+1.5}{\int_{-\infty }^{\frac{x-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}\operatorname{d}x}-a\sigma \int_{k-0.5}^{k+0.5}{\int_{-\infty }^{\frac{x-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}\operatorname{d}x} \\
%  & \approx a\sigma \left( \int_{-\infty }^{\frac{k+1-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}-\int_{-\infty }^{\frac{k-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'} \right)=a\sigma \int_{\frac{k-{{u}_{r}}}{\sigma }}^{\frac{k+1-{{u}_{r}}}{\sigma }}{\operatorname{n}\left( {{u}'} \right)\operatorname{d}{u}'}\approx a\cdot \operatorname{n}\left( \frac{k+0.5-{{u}_{r}}}{\sigma } \right) \\
% \end{align}\]
%
% References
% # 张虎, 达飞鹏, 邢德奎: 光学测量中椭圆圆心定位算法研究

if nargin < 6
    gaussFitMethod = int8(1);
end
if nargin < 5
    grayScaleDiffMethod = int8(1);
end
if nargin < 4
    nNeighborPts = 3;
end

nEdges = size(edges0, 3);
[yRes, xRes] = size(I);

edge = -ones(size(edges0));
for iEdge=1:nEdges
    thisEdge0 = edges0(:, :, iEdge);
    thisEdge0(any(thisEdge0<=0, 2), :) = [];    
    
    for iEdgePt=1:size(thisEdge0, 1)
        thisEdgePt = double(thisEdge0(iEdgePt, :)'); % 当前边缘点
        partDeriv = [2*coef(nEdges, 1)*thisEdgePt(1, 1)+coef(nEdges, 2)*thisEdgePt(2, 1)+coef(nEdges, 4); ...
            2*coef(nEdges, 3)*thisEdgePt(2, 1)+coef(nEdges, 2)*thisEdgePt(1, 1)+coef(nEdges, 5)];
        k = partDeriv(2, 1) / partDeriv(1, 1);
        
        % 灰度插值及灰度差求取
        ind = NaN(nNeighborPts*2+1, 1);
        grayScale = NaN(nNeighborPts*2+1, 1);
        GSDiff = NaN(nNeighborPts*2+1, 1);
        ind(nNeighborPts+1, 1) = 0;
        pNb = NaN(2, 1);
        grayScale(nNeighborPts+1, 1) = I(thisEdgePt(1, 1), thisEdgePt(2, 1));%matlab和opencv提取的点坐标横纵坐标相反
        if abs(k) < 1 % 边缘点在区域1内，梯度方向更靠近X轴 
            for iNb = -nNeighborPts:nNeighborPts
                if iNb ~= 0
                    pNb(1, 1) = thisEdgePt(1, 1) + iNb;
                    pNb(2, 1) = thisEdgePt(2, 1) + iNb*k;
                    if all((pNb>=[1; 1]) & (pNb<=[xRes; yRes])) % 邻近点在图像范围内
                        y0 = floor(pNb(2, 1));
                        if (y0>=1) && (y0+1<=yRes)
                            lambda = pNb(2, 1) - y0;
                            ind(iNb+nNeighborPts+1, 1) = iNb;
                            grayScale(iNb+nNeighborPts+1, 1) = (1-lambda)*double(I(pNb(1, 1), y0)) + lambda*double(I(pNb(1, 1), y0+1));
                        end
                    end
                end
            end
        else % 边缘点在区域2内，梯度方向更靠近Y轴
            for iNb = -nNeighborPts:nNeighborPts
                if iNb ~= 0
                    pNb(1, 1) = thisEdgePt(1,1) + iNb/k;
                    pNb(2, 1) = thisEdgePt(2,1) + iNb;
                    if all((pNb>=[1; 1]) & (pNb<=[xRes; yRes])) % 邻近点在图像范围内
                        x0 = floor(pNb(1, 1));
                        if (x0>=1) && (x0+1<=xRes)
                            lambda = pNb(1, 1) - x0;
                            ind(iNb+nNeighborPts+1, 1) = iNb;
                            grayScale(iNb+nNeighborPts+1, 1) = (1-lambda)*double(I(x0, pNb(2, 1))) + lambda*double(I( x0+1,pNb(2, 1)));
                        end
                    end
                end
            end
        end
        
        if grayScaleDiffMethod == int8(1) % 使用原论文算法计算灰度差
            GSDiff(1, 1) = abs(grayScale(2, 1) - grayScale(1, 1));
            ind(1, 1) = ind(1, 1) + 0.5;
            GSDiff(end, 1) = abs(grayScale(end, 1) - grayScale(end-1, 1));
            ind(end, 1) = ind(end, 1) - 0.5;
            for iNb = 2:(nNeighborPts*2)
                GSDiff(iNb, 1) = (abs(grayScale(iNb+1, 1)-grayScale(iNb, 1)) + abs(grayScale(iNb, 1)-grayScale(iNb-1, 1))) / 2;
            end
        else % 使用相邻元素计算灰度差，将不符合整体灰度变化方向的点标识为无效点
            GSDiff = diff(grayScale);
            ind = ind(1:(end-1), 1) + 0.5;
            signIsDiffWithMajorSign = (sign(GSDiff)~=sign(grayScale(end, 1)-grayScale(1, 1)));
            GSDiff(signIsDiffWithMajorSign) = NaN;
            ind(signIsDiffWithMajorSign) = NaN;
        end
        
        % 高斯曲线拟合
        ind(isnan(GSDiff)) = []; % 去掉无效点
        GSDiff(isnan(GSDiff)) = [];
        if gaussFitMethod == int8(1) % 使用原论文算法进行拟合
            c = intcoeffs(ind+0.5) - intcoeffs(ind-0.5);
            ABC = c \ GSDiff;
            delta = -ABC(2, 1) / (2*ABC(1, 1)); % mu=-B/(2*A), sigma=sqrt(-1/(2*A))
        else % 使用非线性最小二乘进行拟合
            p = lsqcurvefit(@gauss, [0 1 1], ind, GSDiff); % TODO: 预估p初始值
            delta = p(1) - 0.5;
        end
        deltax = delta / sqrt(k^2+1);
        deltay = sign(k) * k * deltax;
        edge(iEdgePt, :, iEdge) = thisEdgePt' + [deltax, deltay];
    end
end
end

function [coef] = intcoeffs(d)
coef = [d.^3/3, d.^2/2, d]; % A*d^2 + B*d + C的原函数为(A*d^3)/3 + (B*d^2)/2 + C*d
end

function [y] = gauss(p, k)
ur = p(1);
sigma = p(2);
a = p(3);
x = (k + 0.5 - ur) ./ sigma;
y = a * 1/sqrt(2*pi) * exp(-x.^2/2);
end